#include "../BSP/can.h"

CAN_TxHeaderTypeDef TXHeader;
CAN_RxHeaderTypeDef RXHeader;

#ifdef DEBUG
uint8_t RXmessage[8];
#else
static uint8_t RXmessage[8];
#endif
static uint32_t pTxMailbox = 0;
static bool data_ready = false;

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
  if (hcan->Instance == CAN1) {
    HAL_CAN_GetRxMessage(hcan, CAN_FILTER_FIFO0, &RXHeader, RXmessage);
    data_ready = true;
  }
}

void BSP_CAN_SendData(CAN_HandleTypeDef *hcan, uint32_t id, uint8_t length,
                      uint8_t *data) {
  TXHeader.StdId = id;
  TXHeader.ExtId = 0x0000;
  TXHeader.DLC = length;
  TXHeader.IDE = CAN_ID_STD;
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.TransmitGlobalTime = DISABLE;
  HAL_CAN_AddTxMessage(hcan, &TXHeader, data, &pTxMailbox);
}

void BSP_CAN_SetFilter(CAN_HandleTypeDef *hcan, uint32_t id) {
  CAN_FilterTypeDef filter;
  filter.FilterActivation = ENABLE;
  filter.FilterBank = 0;
  filter.FilterMode = CAN_FILTERMODE_IDLIST;
  filter.FilterScale = CAN_FILTERSCALE_32BIT;
  filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;

  filter.FilterIdHigh = id << 5;
  filter.FilterIdLow = 0x0000;
  filter.FilterMaskIdHigh = 0x0000;
  filter.FilterMaskIdLow = 0x0000;
  if (HAL_CAN_ConfigFilter(hcan, &filter) != HAL_OK) {
    Error_Handler();
  }
}

void BSP_CAN_Init(CAN_HandleTypeDef *hcan) {
  if (HAL_CAN_Start(hcan) != HAL_OK) {
    Error_Handler();
  }
  if (HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING) !=
      HAL_OK) {
    Error_Handler();
  }
}

bool BSP_CAN_DataReady() { return data_ready; }

uint8_t *BSP_CAN_GetData() { return RXmessage; }
